#include <string.h>
#include <pspdisplay.h>
#include <pspge.h>
#include "video.h"
#include "io_util.h"
#include "main.h"
#include "memory.h"
#include "shin_log.h"
#include "rmff_queue.h"
//#include "libavcodec/rv34.h"
#include "shin.h"

#ifdef video_enable

#define FRAME_BUFFER_WIDTH 512
#define FRAME_BUFFER_HEIGHT 272
#define FRAME_BUFFER_SIZE FRAME_BUFFER_WIDTH * FRAME_BUFFER_HEIGHT * 4

#define min(x, y) (x < y? x: y)

int gcd(int x, int y)
{
	while(y)
	{
		x %= y;
		x ^= y;
		y ^= x;
		x ^= y;
	}
	return x;
}

void calc_scale_dimension(int w, int h, int *sw, int *sh)
{
	int ratio = gcd(w, h);
	w /= ratio; h /= ratio;
	ratio = min(480 / w, 272 / h);
	*sw = w *ratio; *sh = h * ratio;
}

const int csY_coeff_16 = 1.164383*(1<<16);
const int csU_blue_16  = 2.017232*(1<<16);
const int csU_green_16 = (-0.391762)*(1<<16); 
const int csV_green_16 = (-0.812968)*(1<<16);
const int csV_red_16   = 1.596027*(1<<16);

void ZoomCsc(AVFrame *frame, int width_ratio, int height_ratio, \
             int scale_width, int scale_height, uint8_t *dst)
{
	__asm__ __volatile__
	(
		".set push\n\t"
		".set noreorder\n\t"
		"addi $sp,$sp,-8\n\t"
		"li $25,0xff\n\t"
//$25 = 0xff
		"move $14,%10\n\t"
//$14 = dst
		"move $2,$0\n\t"
//i = 0
		"sub $14,$14,1\n\t"
//dst -= 1
		"mult $0,$0\n\t"
//ii = 0
"1:\n\t"
		"sw $2,0($sp)\n\t"
		"lw $8,0(%0)\n\t"
//frame->data[0]
		"lw $9,4(%0)\n\t"
//frame->data[1]
		"lw $10,16(%0)\n\t"
//frame->linesize[0]
		"mflo $2\n\t"
		"sra $2,$2,16\n\t"
//$2 = ii
		"mult $2,$10\n\t"
//ii * linesize[0]
		"sra $2,$2,1\n\t"
//ii >>= 1
		"lw $10,20(%0)\n\t"
//frame->linesize[1]
		"ori $14,$14,0x7ff\n\t"
		"addi $14,$14,1\n\t"
//dst = (void*)((uint32_t)dst | 0x7ff) + 1;
		"mflo $3\n\t"
		"mult $2,$10\n\t"
//ii * linesize[1]
		"addu $8,$8,$3\n\t"
//data[0] + ii * linesize[0]
		"lw $10,8(%0)\n\t"
//data[2]
		"move $3,$0\n\t"
//j = 0
		"mflo $2\n\t"
		"addu $9,$9,$2\n\t"
		"addu $10,$10,$2\n\t"
"2:\n\t"
		"mult $3,%6\n\t"
		"sw $3,4($sp)\n\t"
		"mflo $3\n\t"
		"sra $3,$3,16\n\t"
		"addu $2,$8,$3\n\t"
		"lbu $11,0($2)\n\t"
		"sra $3,$3,1\n\t"
		"addi $11,$11,-16\n\t"
		"mult $11,%1\n\t"
		"addu $2,$9,$3\n\t"
		"lbu $12,0($2)\n\t"
		"addi $12,-128\n\t"
		"addu $2,$10,$3\n\t"
		"mflo $11\n\t"
		"madd %3,$12\n\t"
		"lbu $13,0($2)\n\t"
		"addi $13,-128\n\t"
		"madd %4,$13\n\t"
		"lui $24,0xff00\n\t"
		"mflo $2\n\t"
//g
		"mult %2,$13\n\t"
		"sra $2,$2,16\n\t"
		"max $2,$2,$0\n\t"
		"min $2,$2,$25\n\t"
		"ins $24,$2,8,8\n\t"
		"mflo $2\n\t"
		"mult %5,$12\n\t"
		"add $2,$2,$11\n\t"
//r
		"sra $2,$2,16\n\t"
		"max $2,$2,$0\n\t"
		"min $2,$2,$25\n\t"
		"ins $24,$2,0,8\n\t"
		"mflo $2\n\t"
		"add $2,$2,$11\n\t"
//b
		"sra $2,$2,16\n\t"
		"max $2,$2,$0\n\t"
		"min $2,$2,$25\n\t"
		"ins $24,$2,16,8\n\t"

		"lw $3,4($sp)\n\t"
		"addiu $3,$3,1\n\t"
		"sw $24,0($14)\n\t"
		"bne $3,%8,2b\n\t"
		"addiu $14,$14,4\n\t"

		"lw $2,0($sp)\n\t"
		"addiu $2,$2,1\n\t"
		"bne $2,%9,1b\n\t"
		"mult $2,%7\n\t"
		"addi $sp,$sp,8\n\t"
		".set pop\n\t"
		:
		:"r"(frame), \
		 "r"(csY_coeff_16), "r"(csV_red_16), "r"(csU_green_16), "r"(csV_green_16), "r"(csU_blue_16), \
		 "r"(width_ratio), "r"(height_ratio), "r"(scale_width), "r"(scale_height), "r"(dst)
		:"$2", "$3", "$8", "$9", "$10", "$11", "$12", "$13", "$14", "$24", "$25", "$sp"
	);
/*
	int i, ii, j, jj, y, u, v, r, g ,b;
	uint8_t *srcY, *srcU, *srcV;

	for(i = 0; i < scale_height; i++)
	{
		ii = i * height_ratio >> 16;
		srcY = frame->data[0] + ii * frame->linesize[0];
		ii >>= 1;
		srcU = frame->data[1] + ii * frame->linesize[1];
		srcV = frame->data[2] + ii * frame->linesize[2];
		for(j = 0; j < scale_width; j++)
		{
			jj = j * width_ratio >> 16;
			y = (srcY[jj] - 16) * csY_coeff_16;
			jj >>= 1;
			u = srcU[jj] - 128;
			v = srcV[jj] - 128;
			r = y + csV_red_16 * v;
			g = y + csU_green_16 * u + csV_green_16 * v;
			b = y + csU_blue_16 * u;
			__asm__ __volatile__(
				".set push\n\t"
				".set noreorder\n\t"
				"li $8,0xff\n\t"
				"lui $9,0xff00\n\t"
				"sra %1,%1,16\n\t"
				"max %1,%1,$0\n\t"
				"min %1,%1,$8\n\t"
				"ins $9,%1,16,8\n\t"
				"sra %2,%2,16\n\t"
				"max %2,%2,$0\n\t"
				"min %2,%2,$8\n\t"
				"ins $9,%2,8,8\n\t"
				"sra %3,%3,16\n\t"
				"max %3,%3,$0\n\t"
				"min %3,%3,$8\n\t"
				"ins $9,%3,0,8\n\t"
				"sw $9,0(%4)\n\t"
				"addiu %0,%4,4\n\t"
				".set pop\n"
				:"=r"(dst)
				:"r"(b), "r"(g), "r"(r), "r"(dst)
				:"$8","$9"
			);
		}
		dst = (void*)((uint32_t)dst | 0x7ff) + 1;
	}
*/
}

//#define INFINITE
#define NUM 5 * 23
int count = 0;
AVCodecContext rv40_ctx __attribute__((aligned(16)));
uint8_t *vram;
uint32_t sum;
AVCodec *codec;
rmff_queue_t video_queue;

int VideoPlaybackThread(SceSize args, void *argp)
{
	rmff_packet_t *packet;
	AVFrame *frame;
	int got, scale_width, scale_height;
	uint32_t width_ratio;
	uint32_t height_ratio;

	debug_log("video playback thread started");
	avcodec_register_all();
	codec = avcodec_find_decoder(CODEC_ID_RV40);
	if(!codec)fatal_log("find decoder error");
	debug_log("decoder found");
	if(avcodec_open(&rv40_ctx, codec) < 0)fatal_log("open decoder error");
	calc_scale_dimension(rv40_ctx.width, rv40_ctx.height, &scale_width, &scale_height);
	width_ratio = (rv40_ctx.width << 16) / scale_width;
	height_ratio = (rv40_ctx.height << 16) / scale_height;
	debug_log("width: %d, height: %d", scale_width, scale_height);
	frame = avcodec_alloc_frame();
//	buffer = shin_malloc(avpicture_get_size(PIX_FMT_RGBA, scale_width, scale_height));
//	avpicture_alloc(&scale_pic, PIX_FMT_RGBA, scale_width, scale_height);
//	avpicture_fill(&scale_pic, buffer, PIX_FMT_RGBA, scale_width, scale_height);
#ifdef INFINITE
	while(1)
#else
	while(++count <= NUM)
#endif
	{
		got = 0;
		while(!got)
		{
			packet = rmff_queue_get_packet(&video_queue);
//			ff_rv34_decode_frame(&rv40_ctx, &frame, &size, &pkt);
            if(count%10==0)
            rv40_ctx.hurry_up=4;
            else rv40_ctx.hurry_up=6;
			avcodec_decode_video(&rv40_ctx, frame, &got, packet->data, packet->length);
			if(count%10==0)
			rmff_release_packet(packet);
		}
		if(!packet)
		continue;
		if(count%10==0)
		{
		memset(vram, 0, FRAME_BUFFER_SIZE);
		ZoomCsc(frame, width_ratio, height_ratio, scale_width, scale_height, vram);
		sceDisplaySetFrameBuf(vram, FRAME_BUFFER_WIDTH, PSP_DISPLAY_PIXEL_FORMAT_8888, \
	                          PSP_DISPLAY_SETBUF_IMMEDIATE);
//		sceKernelSleepThread();
		vram = (uint8_t*)(sum - (uint32_t)vram);
      }
//		for(i = 0; i < scale_height; i++)
//			memcpy(vram + i * FRAME_BUFFER_WIDTH * 4, scale_pic.data[0] + scale_pic.linesize[0] * i, 4 * scale_width);
	}
	debug_log("all ok");
//	gprof_cleanup();
	MyExit();
	return 0;
}
#endif

//extern const AVClass av_codec_context_class;
//#include "libavcodec/opt.h"

SceUID video_init(rmff_stream_t *stream)
{
#ifdef video_enable
	SceUID tid;

	vram = (void*)(0x40000000 | (uint32_t)sceGeEdramGetAddr());
	sum = (uint32_t)(vram) * 2 + FRAME_BUFFER_SIZE;
	sceDisplaySetFrameBuf(vram + FRAME_BUFFER_SIZE, FRAME_BUFFER_WIDTH, PSP_DISPLAY_PIXEL_FORMAT_8888, \
	                      PSP_DISPLAY_SETBUF_IMMEDIATE);
	rmff_queue_init(&video_queue, video_queue_size);
	stream->queue = &video_queue;
	memset(&rv40_ctx, 0, sizeof(AVCodecContext));
//	rv40_ctx.av_class= &av_codec_context_class;
	rv40_ctx.codec_type = CODEC_TYPE_VIDEO;
//	av_opt_set_defaults2(&rv40_ctx, AV_OPT_FLAG_VIDEO_PARAM, AV_OPT_FLAG_VIDEO_PARAM);
    rv40_ctx.get_buffer= avcodec_default_get_buffer;
    rv40_ctx.release_buffer= avcodec_default_release_buffer;
	rv40_ctx.bit_rate = stream->avg_bit_rate;
	rv40_ctx.width = stream->width;
	rv40_ctx.height = stream->height;
	rv40_ctx.thread_count = 1;
	tid = sceKernelCreateThread("video playback thread", VideoPlaybackThread, \
	                            video_playback_priority, video_playback_stack_size, \
								0, 0);
	sceKernelStartThread(tid, 0, 0);
	return tid;
#else
	return 0;
#endif
}

#ifdef video_enable
uint8_t rmff_video_get_num(uint16_t *data, uint32_t *num)
{
    int n;
	make_16_be(data);
	n = *data & 0x7fff;
    if(n & 0x4000)
	{
		*num = n & 0x3fff;
		return 2;
	}
    else
	{
		make_16_be(++data);
		*num = (n << 16) | *data;
		return 4;
    }
}
#endif

	static rmff_packet_t *frame_pkt;
	static uint32_t frame_rest = 0;
	static uint32_t frame_len = 0;
	static uint32_t frame_off = 0;
	static uint8_t frame_seq = 0;
	static uint8_t frame_sub = 0;
	static uint8_t frame_total = 0;

void rv40_put_packet(rmff_queue_t *queue, rmff_packet_t *packet)
{
#ifdef video_enable
	rmff_packet_t *pkt;
	uint32_t total_len, sub_off, rest_len, hdr_size;
	uint8_t *p, type, hdr, total, sub, seq;

	if(packet->length <= 2)goto rv40_over;
	p = packet->data;
	hdr = *p; p++;
	type = hdr >> 6;
	if(!(type & 1))
	{
		if(packet->length <= 7)goto rv40_over;
		sub = *p++;
		total = hdr << 1 | sub >> 7;
		sub &= 0x7f;
		hdr_size = 8 * total + 1;
		p += rmff_video_get_num((void*)p, &total_len);
		p += rmff_video_get_num((void*)p, &sub_off);
/*		asm(
		"addiu %1,%2,1\n"
		"lbu %0,0(%2)\n"
		:"=r"(seq),"=r"(p)
		:"r"(p)
		);
*/
		seq = *p++;
		rest_len = packet->length;
		rest_len -= (void*)p - packet->data;
		if(frame_pkt)
			if(total_len != frame_len || total != frame_total || sub != frame_sub || frame_seq != seq || \
			   (type & 2 && \
			    ( \
			     (sub_off != frame_rest) || \
//it seems that some last partial packet has padding
			     (rest_len < frame_rest) || \
			     (sub != total) \
				) \
			   ) || \
			   (!(type & 2) && \
			    ( \
				 (sub_off != frame_off) || \
			     (rest_len > frame_rest) || \
//it seems that some frame don't let the last partial has the type 10
			     (sub > total) \
				) \
			   ) \
			  )
			{
				error_log("frame error");
				rmff_release_packet(frame_pkt);
				frame_pkt = 0;
			}
		if(!frame_pkt)
			if(total_len && !sub_off)
			{
				frame_total = total; frame_sub = 1; frame_off = 0; frame_seq = seq;
				frame_pkt = shin_alloc_packet();
				frame_pkt->stream = packet->stream;
				frame_pkt->timestamp = packet->timestamp;
				frame_pkt->length = hdr_size + (frame_len = frame_rest = total_len);
				frame_pkt->data = shin_mallocz(frame_pkt->length);
				*(uint8_t*)frame_pkt->data = total - 1;
			}
			else
				goto rv40_over;
		else;
		((uint8_t*)frame_pkt->data)[sub * 8 - 7] = 1;
		((uint8_t*)frame_pkt->data)[sub * 8 - 3] = frame_off & 0xFF;
		((uint8_t*)frame_pkt->data)[sub * 8 - 2] = frame_off >> 8 & 0xFF;
		((uint8_t*)frame_pkt->data)[sub * 8 - 1] = frame_off >> 16 & 0xFF;
		((uint8_t*)frame_pkt->data)[sub * 8] = frame_off >> 24 & 0xFF;
		memcpy(frame_pkt->data + hdr_size + frame_off, p, min(rest_len, frame_rest));
		frame_rest -= rest_len;
		frame_off += rest_len;
		if(total == frame_sub++)
		{
			rmff_queue_tail_add(queue, frame_pkt);
			frame_pkt = 0;
		}
	}
	else if(type == 1)
	{
		if(frame_pkt)
		{
			warning_log("last frame seems not over");
			rmff_release_packet(frame_pkt);
			frame_pkt = 0;
		}
		frame_rest = 0;
		frame_off = 0;
		frame_seq++;
		pkt = shin_alloc_packet();
		pkt->stream = packet->stream;
		pkt->timestamp = packet->timestamp;
		pkt->length = packet->length - 2;
		pkt->data = shin_malloc(pkt->length + 9);
		*(uint8_t*)(frame_pkt->data + 1) = 1;
		memcpy(pkt->data + 9, packet->data + 2, pkt->length);
		rmff_queue_tail_add(queue, pkt);
	}
	else
		warning_log("skip type 3 multiframe");
rv40_over:
#endif
	rmff_release_packet(packet);
}
